#include <iostream>
#include "opencv2/highgui/highgui_c.h"
#include "opencv2/imgproc/imgproc_c.h"
#include<opencv2/opencv.hpp>
#include "find_color.hpp"


int find_goal(cv::Mat frame,int cls)
{
	int iLowH = 100;     
	int iHighH = 137;
		
	int iLowS = 138;
	int iHighS = 255;
		
	int iLowV = 0;
	int iHighV = 255;//（22，56）（110，255）（30，255）

	cv::Mat mask,image;
    cv::Rect r1(0, 240, 640, 240);        //掩膜区域
    mask = Mat::ones(frame.size(), CV_8UC1);
    mask(r1).setTo(0);
	frame.copyTo(image, mask);   //frame和mask与运算后赋给image

	switch(cls)
	{
		case 0:		//bottle（蓝色）
		{
			iLowH = 100;     
			iHighH = 137;
		
			iLowS = 138;
			iHighS = 255;
		
			iLowV = 0;
			iHighV = 255;
			break;
		}
		case 1:		//battery(红色)
		{
			iLowH = 142;    
			iHighH = 179;
		
			iLowS = 65;
			iHighS = 255;
		
			iLowV = 0;
			iHighV = 255;
			break;
		}
		case 2:		//paper(蓝色)
		{
			iLowH = 100;     
			iHighH = 137;
		
			iLowS = 138;
			iHighS = 255;
		
			iLowV = 0;
			iHighV = 255;
			break;
		}
		case 3:			//orange(绿色)
		{
			iLowH = 35;        
			iHighH = 77;
		
			iLowS = 210;
			iHighS = 255;
		
			iLowV = 46;
			iHighV = 255;
			break;
		}
		case 4:				//纸杯(灰色)
		{
			iLowH = 95;        
			iHighH = 128;
		
			iLowS = 27;
			iHighS = 89;
		
			iLowV = 0;
			iHighV = 255;
			break;
		}
		default : // 可选的
			break;
	}

	namedWindow("Control", CV_WINDOW_AUTOSIZE); //create a window called "Control"
 
	Mat imgHSV;
	cvtColor(image, imgHSV, COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV
 
	Mat imgThresholded;
	inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); //二值化取出对应颜色区域
 
	//开操作 (去除一些噪点)																							  
	Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
	morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);      

	//闭操作 (连接一些连通域)
	morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);
        
    // 模糊化
    Mat gaussian;
    GaussianBlur(imgThresholded, gaussian,Size(3,3), 0, 0);

    vector<vector<Point> > contours;
    vector<Vec4i> hierarchy;

    findContours(gaussian, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);	//寻找图像中对应色彩区间的的所有边框

    double maxarea = 0;
    int maxareaidx = -1;
    for (int index = contours.size() - 1; index >= 0; index --)// 寻找最大面积
    {
        double tmparea = fabs(contourArea(contours[index]));
        if (tmparea > maxarea)
        {
            maxarea = tmparea;
            maxareaidx = index;
        }
    }

	RotatedRect box;
	Point2f rect[4];

	float center_x;
	if (maxareaidx >= 0)
	{	
		box = minAreaRect(Mat(contours[maxareaidx]));  //计算轮廓的最小外接矩形
		// boundRect = boundingRect(Mat(contours[maxareaidx]));
		Mat boxPts;			
		boxPoints(box, boxPts);		// 得到矩形的四个顶点坐标(左下角开始顺时针)
		// cout<< boxPts << endl;

		box.points(rect);  //把最小外接矩形四个端点复制给rect数组
		
		center_x = (rect[0].x + rect[3].x)/2.0;		//中心位置x坐标

		// cout<< (Point)rect[0].x << endl;
		// cout<< (Point)rect[1] << endl;
		// cout<< (Point)rect[2] << endl;
		// cout<< (Point)rect[3] << endl;

		// rectangle(frame, Point(boundRect.x, boundRect.y), Point(boundRect.x + 
		// boundRect.width, boundRect.y + boundRect.height), Scalar(0, 255, 0), 2, 8);
		for(int j=0; j<4; j++)
			line(image, rect[j], rect[(j+1)%4], Scalar(0, 0, 255), 2, 8);  //绘制最小外接矩形每条边
	}    
	// imshow("Thresholded Image", imgThresholded); //二值化结果经过开操作和闭操作
	imshow("frame", image); //原始图片
	return (int)center_x;
}





